/* Siconos is a program dedicated to modeling, simulation and control
 * of non smooth dynamical systems.
 *
 * Copyright 2024 INRIA.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */
#include <assert.h>  // for assert
#include <float.h>   // for DBL_EPSILON
#include <stdio.h>   // for NULL
#include <string.h>  // for NULL

#include "FrictionContactProblem.h"                    // for FrictionContac...
#include "Friction_cst.h"                              // for SICONOS_FRICTI...
#include "NonSmoothDrivers.h"                          // for fc3d_driver
#include "NumericsFwd.h"                               // for SolverOptions
#include "SolverOptions.h"                             // for SolverOptions
#include "fc3d_Solvers.h"                              // for fc3d_ACLMFixed...
#include "fc3d_nonsmooth_Newton_AlartCurnier.h"        // for fc3d_nonsmooth...
#include "fc3d_nonsmooth_Newton_FischerBurmeister.h"   // for fc3d_nonsmooth...
#include "fc3d_nonsmooth_Newton_natural_map.h"         // for fc3d_nonsmooth...
#include "fc3d_onecontact_nonsmooth_Newton_solvers.h"  // for fc3d_onecontac...
#include "fc3d_projection.h"                           // for fc3d_projectio...
#include "fc3d_unitary_enumerative.h"                  // for fc3d_unitary_e...
#include "numerics_verbose.h"                          // for numerics_printf

const char* const SICONOS_FRICTION_3D_NSGS_STR = "FC3D_NSGS";
const char* const SICONOS_FRICTION_3D_NSGSV_STR = "FC3D_NSGSV";
const char* const SICONOS_FRICTION_3D_TFP_STR = "FC3D_TFP";
const char* const SICONOS_FRICTION_3D_PFP_STR = "FC3D_PFP";
const char* const SICONOS_FRICTION_3D_NSN_AC_STR = "FC3D_NSN_AC";
const char* const SICONOS_FRICTION_3D_NSN_AC_TEST_STR = "FC3D_NSN_AC_TEST";
const char* const SICONOS_FRICTION_3D_NSN_FB_STR = "FC3D_NSN_FB";
const char* const SICONOS_FRICTION_3D_NSN_NM_STR = "FC3D_NSN_NM";
const char* const SICONOS_FRICTION_3D_DSFP_STR = "FC3D_DeSaxceFixedPoint";
const char* const SICONOS_FRICTION_3D_NCPGlockerFBFixedPoint_STR =
    "FC3D_NCPGlockerFBFixedPoint";
const char* const SICONOS_FRICTION_3D_ONECONTACT_NSN_STR = "FC3D_ONECONTACT_NSN";
const char* const SICONOS_FRICTION_3D_ONECONTACT_NSN_GP_STR = "FC3D_ONECONTACT_NSN_GP";
const char* const SICONOS_FRICTION_3D_ONECONTACT_NSN_GP_HYBRID_STR =
    "FC3D_ONECONTACT_NSN_GP_HYBRID";
const char* const SICONOS_FRICTION_3D_NCPGlockerFBNewton_STR = "FC3D_NCPGlockerFBNewton";
const char* const SICONOS_FRICTION_3D_ONECONTACT_ProjectionOnConeWithDiagonalization_STR =
    "FC3D_ProjectionOnConeWithDiagonalization";
const char* const SICONOS_FRICTION_3D_ONECONTACT_ProjectionOnCone_STR =
    "FC3D_ProjectionOnCone";
const char* const SICONOS_FRICTION_3D_ONECONTACT_ProjectionOnConeWithLocalIteration_STR =
    "FC3D_ProjectionOnConeWithLocalIteration";
const char* const SICONOS_FRICTION_3D_ONECONTACT_ProjectionOnConeWithRegularization_STR =
    "FC3D_projectionOnConeWithRegularization";
const char* const SICONOS_FRICTION_3D_NCPGlockerFBPATH_STR = "FC3D_NCPGlockerFBPATH";
const char* const SICONOS_FRICTION_3D_ONECONTACT_ProjectionOnCylinder_STR =
    "FC3D_projectionOnCylinder";
const char* const SICONOS_FRICTION_3D_ONECONTACT_ProjectionOnCylinderWithLocalIteration_STR =
    "FC3D_projectionOnCylinderWithLocalIteration";
const char* const SICONOS_FRICTION_3D_ONECONTACT_ProjectionOnCone_velocity_STR =
    "FC3D_ProjectionOnCone_velocity";
const char* const SICONOS_FRICTION_3D_CONVEXQP_PG_CYLINDER_STR = "FC3D ConvexQP PG solver";
const char* const SICONOS_FRICTION_3D_VI_FPP_Cylinder_STR = "FC3D_VI_FPP_Cylinder";

const char* const SICONOS_FRICTION_3D_EG_STR = "FC3D_ExtraGradient";
const char* const SICONOS_FRICTION_3D_FPP_STR = "FC3D_FixedPointProjection";
const char* const SICONOS_FRICTION_3D_VI_EG_STR = "FC3D_VI_ExtraGradient";
const char* const SICONOS_FRICTION_3D_VI_FPP_STR = "FC3D_VI_FixedPointProjection";
const char* const SICONOS_FRICTION_3D_HP_STR = "FC3D_HyperplaneProjection";
const char* const SICONOS_FRICTION_3D_PROX_STR = "FC3D_PROX";
const char* const SICONOS_FRICTION_3D_GAMS_PATH_STR = "FC3D_GAMS_PATH";
const char* const SICONOS_FRICTION_3D_GAMS_PATHVI_STR = "FC3D_GAMS_PATHVI";
const char* const SICONOS_FRICTION_3D_GAMS_LCP_PATH_STR = "FC3D_GAMS_LCP_PATH";
const char* const SICONOS_FRICTION_3D_GAMS_LCP_PATHVI_STR = "FC3D_GAMS_LCP_PATHVI";
const char* const SICONOS_FRICTION_3D_ONECONTACT_QUARTIC_STR = "FC3D_QUARTIC";
const char* const SICONOS_FRICTION_3D_ONECONTACT_QUARTIC_NU_STR = "FC3D_QUARTIC_NU";
const char* const SICONOS_FRICTION_3D_ACLMFP_STR = "FC3D_ACLMFP";
const char* const SICONOS_FRICTION_3D_SOCLCP_STR = "FC3D_SOCLCP";

int fc3d_driver(FrictionContactProblem* problem, double* reaction, double* velocity,
                SolverOptions* options) {
  if (options == NULL) numerics_error("fc3d_driver", "null input for solver options");

  assert(options->isSet); /* true(1) if the SolverOptions structure has been filled in else
                             false(0) */

  if (verbose > 1) solver_options_print(options);

  int info = -1;

  if (problem->dimension != 3)
    numerics_error(
        "fc3d_driver",
        "Dimension of the problem : problem-> dimension is not compatible or is not set");

  /* Check for trivial case */
  info = fc3d_checkTrivialCase(problem, velocity, reaction, options);
  if (info == 0) {
    /* If a trivial solution is found, we set the number of iterations to 0
       and the reached acuracy to 0.0 .
    */
    options->iparam[SICONOS_IPARAM_ITER_DONE] = 0;
    options->dparam[SICONOS_DPARAM_RESIDU] = 0.0;
    goto exit;
  }

  switch (options->solverId) {
    /* Non Smooth Gauss Seidel (NSGS) */
    case SICONOS_FRICTION_3D_NSGS: {
      numerics_printf(
          " ========================== Call NSGS solver for Friction-Contact 3D problem "
          "==========================\n");
      fc3d_nsgs(problem, reaction, velocity, &info, options);
      break;
    }
    case SICONOS_FRICTION_3D_NSGSV: {
      numerics_printf(
          " ========================== Call NSGSV solver for Friction-Contact 3D problem "
          "==========================\n");
      fc3d_nsgs_velocity(problem, reaction, velocity, &info, options);
      break;
    }
    /* ADMM*/
    case SICONOS_FRICTION_3D_ADMM: {
      numerics_printf(
          " ========================== Call ADMM solver for Friction-Contact 3D problem "
          "==========================\n");
      fc3d_admm(problem, reaction, velocity, &info, options);
      break;
    }
    /* Proximal point algorithm */
    case SICONOS_FRICTION_3D_PROX: {
      numerics_printf(
          " ========================== Call PROX (Proximal Point) solver for Friction-Contact "
          "3D problem ==========================\n");
      fc3d_proximal(problem, reaction, velocity, &info, options);
      break;
    }
    /* Tresca Fixed point algorithm */
    case SICONOS_FRICTION_3D_TFP: {
      numerics_printf(
          " ========================== Call TFP (Tresca Fixed Point) solver for "
          "Friction-Contact 3D problem ==========================\n");
      fc3d_TrescaFixedPoint(problem, reaction, velocity, &info, options);
      break;
    }
    /* Panagiotopoulos Fixed point algorithm */
    case SICONOS_FRICTION_3D_PFP: {
      numerics_printf(
          " ========================== Call PFP (Panagiotopoulos Fixed Point) solver for "
          "Friction-Contact 3D problem ==========================\n");
      fc3d_Panagiotopoulos_FixedPoint(problem, reaction, velocity, &info, options);
      break;
    }
    /* ACLM Fixed point algorithm */
    case SICONOS_FRICTION_3D_ACLMFP: {
      numerics_printf(
          " ========================== Call ACLM (Acary Cadoux Lemarechal Malick Fixed Point) "
          "solver for Friction-Contact 3D problem ==========================\n");
      fc3d_ACLMFixedPoint(problem, reaction, velocity, &info, options);
      break;
    }
    /* SOCLCP Fixed point algorithm */
    case SICONOS_FRICTION_3D_SOCLCP: {
      numerics_printf(
          " ========================== Call SOCLCP solver for Friction-Contact 3D problem "
          "(Associated one) ==========================\n");
      fc3d_SOCLCP(problem, reaction, velocity, &info, options);
      break;
    }
    /* De Saxce Fixed point algorithm */
    case SICONOS_FRICTION_3D_DSFP: {
      numerics_printf(
          " ========================== Call DeSaxce Fixed Point (DSFP) solver for "
          "Friction-Contact 3D problem ==========================\n");
      fc3d_DeSaxceFixedPoint(problem, reaction, velocity, &info, options);
      break;
    }
    /* Fixed point projection algorithm */
    case SICONOS_FRICTION_3D_FPP: {
      numerics_printf(
          " ========================== Call Fixed Point Projection (FPP) solver for "
          "Friction-Contact 3D problem ==========================\n");
      fc3d_fixedPointProjection(problem, reaction, velocity, &info, options);
      break;
    }

    /* Extra Gradient algorithm */
    case SICONOS_FRICTION_3D_EG: {
      numerics_printf(
          " ========================== Call ExtraGradient (EG) solver for Friction-Contact 3D "
          "problem ==========================\n");
      fc3d_ExtraGradient(problem, reaction, velocity, &info, options);
      break;
    }
    /* VI Fixed Point Projection algorithm */
    case SICONOS_FRICTION_3D_VI_FPP: {
      numerics_printf(
          " ========================== Call VI_FixedPointProjection (VI_FPP) solver for "
          "Friction-Contact 3D problem ==========================\n");
      fc3d_VI_FixedPointProjection(problem, reaction, velocity, &info, options);
      break;
    }
    /* VI Extra Gradient algorithm */
    case SICONOS_FRICTION_3D_VI_EG: {
      numerics_printf(
          " ========================== Call VI_ExtraGradient (VI_EG) solver for "
          "Friction-Contact 3D problem ==========================\n");
      fc3d_VI_ExtraGradient(problem, reaction, velocity, &info, options);
      break;
    }
    /* Hyperplane Projection algorithm */
    case SICONOS_FRICTION_3D_HP: {
      numerics_printf(
          " ========================== Call Hyperplane Projection (HP) solver for "
          "Friction-Contact 3D problem ==========================\n");
      fc3d_HyperplaneProjection(problem, reaction, velocity, &info, options);
      break;
    }
    /* Alart Curnier in local coordinates */
    case SICONOS_FRICTION_3D_NSN_AC: {
      numerics_printf(
          " ========================== Call Alart Curnier solver for Friction-Contact 3D "
          "problem ==========================\n");
      fc3d_nonsmooth_Newton_AlartCurnier(problem, reaction, velocity, &info, options);
      break;
    }
    /*  XXX to delete */
    case SICONOS_FRICTION_3D_NSN_AC_TEST: {
      numerics_printf(
          " ========================== Call Alart Curnier solver for Friction-Contact 3D "
          "problem ==========================\n");
      fc3d_nonsmooth_Newton_AlartCurnier2(problem, reaction, velocity, &info, options);
      break;
    }
    /* Fischer Burmeister in local coordinates */
    case SICONOS_FRICTION_3D_NSN_FB: {
      numerics_printf(
          " ========================== Call Fischer Burmeister solver for Friction-Contact 3D "
          "problem ==========================\n");
      fc3d_nonsmooth_Newton_FischerBurmeister(problem, reaction, velocity, &info, options);
      break;
    }
    case SICONOS_FRICTION_3D_NSN_NM: {
      numerics_printf(
          " ========================== Call natural map solver for Friction-Contact 3D "
          "problem ==========================\n");
      fc3d_nonsmooth_Newton_NaturalMap(problem, reaction, velocity, &info, options);
      break;
    }
    case SICONOS_FRICTION_3D_ONECONTACT_QUARTIC_NU:
    case SICONOS_FRICTION_3D_ONECONTACT_QUARTIC: {
      numerics_printf(
          " ========================== Call Quartic solver for Friction-Contact 3D problem "
          "==========================\n");
      fc3d_unitary_enumerative(problem, reaction, velocity, &info, options);
      break;
    }
    case SICONOS_FRICTION_3D_ONECONTACT_NSN:
    case SICONOS_FRICTION_3D_ONECONTACT_NSN_GP:
    case SICONOS_FRICTION_3D_ONECONTACT_NSN_GP_HYBRID: {
      numerics_printf(
          " ========================== Call Newton-based solver for one contact "
          "Friction-Contact 3D problem ==========================\n");
      fc3d_onecontact_nonsmooth_Newton_solvers_initialize(problem, problem, options);
      info = fc3d_onecontact_nonsmooth_Newton_solvers_solve(problem, reaction, options);
      break;
    }
    case SICONOS_FRICTION_3D_ONECONTACT_ProjectionOnConeWithLocalIteration: {
      numerics_printf(
          " ========================== Call Projection on cone solver for one contact "
          "Friction-Contact 3D problem ==========================\n");
      fc3d_projectionOnConeWithLocalIteration_initialize(problem, problem, options);
      info = fc3d_projectionOnConeWithLocalIteration_solve(problem, reaction, options);
      fc3d_projectionOnConeWithLocalIteration_free(problem, problem, options);

      break;
    }
    case SICONOS_FRICTION_3D_GAMS_PATH: {
      numerics_printf(
          " ========================== Call PATH solver via GAMS for an AVI Friction-Contact "
          "3D problem ==========================\n");
      fc3d_AVI_gams_path(problem, reaction, velocity, &info, options);
      break;
    }
    case SICONOS_FRICTION_3D_GAMS_PATHVI: {
      numerics_printf(
          " ========================== Call PATHVI solver via GAMS for an AVI "
          "Friction-Contact 3D problem ==========================\n");
      fc3d_AVI_gams_pathvi(problem, reaction, velocity, &info, options);
      break;
    }
    case SICONOS_FRICTION_3D_GAMS_LCP_PATH: {
      numerics_printf(
          " ========================== Call PATH solver via GAMS for an LCP-based "
          "reformulation of the AVI Friction-Contact 3D problem ==========================\n");
      fc3d_lcp_gams_path(problem, reaction, velocity, &info, options);
      break;
    }
    case SICONOS_FRICTION_3D_GAMS_LCP_PATHVI: {
      numerics_printf(
          " ========================== Call PATHVI solver via GAMS for an LCP-based "
          "reformulation of the AVI Friction-Contact 3D problem ==========================\n");
      fc3d_lcp_gams_pathvi(problem, reaction, velocity, &info, options);
      break;
    }
    default: {
      char msg[200];
      strcpy(msg, "Unknown solver : ");
      strcat(msg, solver_options_id_to_name(options->solverId));
      strcat(msg, "\n");
      numerics_warning("fc3d_driver", msg);
      numerics_error("fc3d_driver", msg);
      info = 1;
    }
  }

exit:
  return info;
}

int fc3d_checkTrivialCase(FrictionContactProblem* problem, double* velocity, double* reaction,
                          SolverOptions* options) {
  /* Number of contacts */
  int nc = problem->numberOfContacts;
  double* q = problem->q;
  /* Dimension of the problem */
  int n = 3 * nc;
  int i = 0;
  /*take off? R=0 ?*/
  for (i = 0; i < nc; i++) {
    if (q[3 * i] < -DBL_EPSILON) return -1;
  }
  for (i = 0; i < n; ++i) {
    velocity[i] = q[i];
    reaction[i] = 0.;
  }

  numerics_printf(
      "fc3d fc3d_checkTrivialCase, take off, trivial solution reaction = 0, velocity = q.\n");
  return 0;
}
